#include "modules/drivers/lidar/orin_hesai/publisher/lidar_publisher.h"

#include <cmath>

#include "cyber/common/global_data.h"

namespace apollo {
namespace drivers {
namespace lidar_hesai {
bool LidarPublisher::Init(
    const std::string pc_channel, std::shared_ptr<Node> node,
    std::shared_ptr<CircularQueueBuffer<PointCloud>> frame_buffer,
    const std::string frame_id) {
  node_ = node;
  pc_channel_ = pc_channel;
  frame_buffer_ = frame_buffer;
  frame_id_ = frame_id;

  lidar_frame_ = std::make_shared<PointCloud>();
  if (node_ == nullptr) {
    AERROR << "Empty node!";
    return false;
  }

  writer_ = node_->CreateWriter<PointCloud>(pc_channel);
  if (writer_ == nullptr) {
    AERROR << "Writer init failed!";
    return false;
  }
  return true;
}

void LidarPublisher::Start() {
  AWARN << "[thread_name]lidar_publish_process";
  AINFO << "start lidar publish process";
  while (!cyber::IsShutdown()) {
    std::this_thread::sleep_for(std::chrono::nanoseconds(10000000));
    if (!frame_buffer_->Get(lidar_frame_.get())) {
      // AINFO << "lidar publish don't get raw frame.";
      continue;
    }
    Write(lidar_frame_);
  }
}

void LidarPublisher::Write(std::shared_ptr<PointCloud>& frame) {
  if (frame->point_size() > 0) {
    auto header = frame->mutable_header();
    header->set_timestamp_sec(cyber::Time::Now().ToSecond());
    header->set_frame_id(frame_id_);
    frame->set_measurement_time(static_cast<double>(header->lidar_timestamp()) / 1e9);
    frame->set_width(frame->point_size());
    frame->set_height(1);
    frame->set_frame_id(frame_id_);
    writer_->Write(frame);
    double time_now = (double)apollo::cyber::Time::Now().ToNanosecond() / 1e9;
    double pointcloud_time = frame->measurement_time();
    double diff = std::fabs(time_now - pointcloud_time);
    static double last_pointcloud_time = 0;

    if (diff > 0.3) {
      AINFO << pc_channel_ << " pointcloud has latency for " << diff
            << ", system time is " << time_now << ", pointcloud time is "
            << pointcloud_time;
    }
    if (last_pointcloud_time != 0 &&
        pointcloud_time - last_pointcloud_time > 0.2) {
      AWARN << pc_channel_ << " pointcloud lost frame, last timestamp is "
            << last_pointcloud_time << ", current timestamp is "
            << pointcloud_time;
    }
    last_pointcloud_time = pointcloud_time;
  }else{
    AERROR << "pointcloud is empty";
  }
}

}  // namespace lidar_hesai
}  // namespace drivers
}  // namespace apollo
